#include "contato.h"

//Código de Contato Original
//Observar se há possibilidade de Cancelamento catastrófico!

void Contato::ForcaResSS(SPHERE* s, SPHERE* t )
{
    REAL Diam, DistCentros, DistCentrosNormalizada;
	REAL Norm[2], Tang[2], Vrel[2];

	Norm[0] = t->R[0] - s->R[0];
	Norm[1] = t->R[1] - s->R[1];
	Diam = s->Rad + t->Rad;
	DistCentros = (Norm[0]*Norm[0]+Norm[1]*Norm[1]);

    double maior=(s->Rad<t->Rad)?s->Rad:t->Rad;
    if (DistCentros<(maior*maior))
    {
        //printf (VERMELHO);
        printf ("\nDistEntreCentros: %f Max:%f\n",sqrt(DistCentros),maior);
        printf ("\nEsfera1: x=%f y=%f",s->R[0],s->R[1]);
        printf ("\nEsfera2: x=%f y=%f",t->R[0],t->R[1]);
        cerr << "\nO modelo Divergiu! A computação foi cancelada.\n";
        printf (AZUL);
        printf ("Diminua o passo!\n");
        printf (BRANCO);
        abort();
    }
    //cout << " Diame="<< Diam*Diam << " Diste="<< DistCentros << ' ';
	if (DistCentros < Diam * Diam)
	{
		REAL rad = 0.5 * HarmonicMean( s->Rad, t->Rad );
		REAL dens = 0.5 * HarmonicMean( s->Mat->Density, t->Mat->Density );
		REAL k = kn * dens * HarmonicMean( s->Mat->Young, t->Mat->Young );

        REAL mass = 0.5 * HarmonicMean( s->Mass, t->Mass );
        //cout << 2*sqrt(mass*kn) << ' ';
		REAL c = cn*(2*sqrt(mass*kn)) * dens * rad * HarmonicMean( s->Mat->Damping, t->Mat->Damping );

		DistCentrosNormalizada = sqrt(DistCentros);
		Norm[0] /= DistCentrosNormalizada;
		Norm[1] /= DistCentrosNormalizada;
		Tang[0] = -Norm[1];
		Tang[1] = Norm[0];
		Vrel[0] = t->V[0] - s->V[0];
		Vrel[1] = t->V[1] - s->V[1];

        REAL Fnorm = k*(Diam-DistCentrosNormalizada) - c * DOT(Vrel,Norm);
		//REAL Fnorm = k*x - c * DOT(Vrel,Norm);

		if (Fnorm > 0.0) {

			REAL Mu_d = Min( s->Mat->Friction_D, t->Mat->Friction_D );
			//REAL mass = 0.5 * HarmonicMean( s->Mass, t->Mass );
			//cout << 1/(1/s->Mass+1/t->Mass) << ' ';
			REAL Vtang = DOT(Vrel,Tang) - t->V[2] * t->Rad - s->V[2] * s->Rad;
			REAL Ftang = SIGN(Vtang) * Min( mass * gamma * Abs(Vtang), Mu_d * Fnorm );

			s->Force( -Fnorm, Norm );
			s->Force( Ftang, Tang );
			s->Torque( Ftang * s->Rad );

			t->Force( Fnorm, Norm );
			t->Force( -Ftang, Tang );
			t->Torque( Ftang * t->Rad );
		}
	}
}

void Contato::ForcaResSL (SPHERE* s, LINE* l)
{
	REAL Dist, Dist2;
	REAL Norm[2], R1[2], R2[2];

	Norm[0] = -l->U[1];							// Vetor normal à U
	Norm[1] = l->U[0];

	R1[0] = s->R[0] - l->R1[0];						// Posição da esfera em relação ao ponto 1 da linha
	R1[1] = s->R[1] - l->R1[1];

	R2[0] = s->R[0] - l->R2[0];						// Posição da esfera em relação ao ponto 2 da linha
	R2[1] = s->R[1] - l->R2[1];

	Dist = DOT( R1, Norm );							// Altura da esfera em relação à linha

	if (Dist < 0.0) {							// Correção da direção da Normal
		Dist = -Dist;
		Norm[0] = -Norm[0];
		Norm[1] = -Norm[1];
	}

	if (Dist < s->Rad) {							// Zona P

		REAL k = 0.5E-4 * s->Mat->Density * HarmonicMean( l->Mat->Young, s->Mat->Young );
		REAL c = 1.5E4 * s->Mat->Density * s->Rad * HarmonicMean( l->Mat->Damping, s->Mat->Damping );

		if (DOT( R1, l->U ) < 0.0) {					// Região 1: R1.U < 0

			Dist2 = DOT( R1, R1 );					// Distância^2 da esfera ao ponto 1 da linha
			if (Dist2 < s->Rad * s->Rad) {				// Teste de interseção
				s->Force( ( k * (s->Rad * sqrt(Dist2) - Dist2) - c * DOT(s->V,R1) ) / Dist2, R1 );
			}
		}
		else if (DOT( R2, l->U ) <= 0.0) {				// Região 2: R1.U >= 0 e R2.U <= 0

			REAL Fnorm = k * (s->Rad - Dist) - c * DOT(s->V,Norm);

			if (Fnorm > 0.0) {

				REAL Mu_d = Min( l->Mat->Friction_D, s->Mat->Friction_D );
				REAL Vtang = DOT( s->V, l->U ) + SIGN(PERP_DOT( R1, l->U )) * s->V[2] * s->Rad;
				REAL Ftang = SIGN(Vtang) * Min( s->Mass * 1000.0 * Abs(Vtang), Mu_d * Fnorm);

				s->Force( Fnorm, Norm );
				s->Force( -Ftang, l->U );
				s->Torque( SIGN(PERP_DOT( R1, l->U )) * Ftang * s->Rad );
			}
		}
		else {								// Região 3: R2.U > 0

			Dist2 = DOT( R2, R2 );					// Distância^2 da esfera ao ponto 2 da linha
			if (Dist2 < s->Rad * s->Rad) {				// Teste de interseção
				s->Force( ( k * (s->Rad * sqrt(Dist2) - Dist2) - c * DOT(s->V,R2) ) / Dist2, R2 );
			}
		}
	}
}
